#include <Servo.h>  //inlude servo library

#define beagle_address 0x48 //NEED TO FIND CORRECT BUS ADDRESS
Servo xServo; //servo object for panning
Servo yServo; //servo object for tilting
int servoSpd = 5; // incremental change of servo position

//servo limits
int xServoMin = 0 + servoSpd;
int xServoMax = 180 - servoSpd;
int yServoMin = 55 + servoSpd;
int yServoMax = 180 - servoSpd;
int xServoCentre = (xServoMin + xServoMax)/2;
int yServoCentre = (yServoMin + yServoMax)/2;

//servo position variables
int xPos = xServoCentre;    // variable to store the servo position 
int yPos = yServoCentre;    // variable to store the servo position 
int servoDelay = 10; //time in milliseconds to wait for servo to move

//robot and camera command variables
int r = 0;
int c = 0;
  
void setup() {
  Serial.begin(9600);  
  
  //setup output pins
  pinMode(22, OUTPUT); //y-direction
  pinMode(23, OUTPUT); //y
  pinMode(24, OUTPUT); //x direction
  pinMode(25, OUTPUT); //x
  pinMode(26, OUTPUT);
  pinMode(27, OUTPUT);
  pinMode(28, OUTPUT);
  pinMode(29, OUTPUT);
  
  //setup servo motors to PWM pins
  xServo.attach(12);  //attach x servo to pin 12
  yServo.attach(13);  //attach y servo to pin 13 
  
  xServo.write(xServoCentre);
  yServo.write(yServoCentre);
}

int getCommand()
{
  int a;
  int i = 1;
    while(i<2)
    {
      if (Serial.available())
      {
        a = Serial.read();  
        i++;
      }
    }
    return a;
}

void loop()
{
  int data = getCommand();
  
  switch (data)
  {
    case 97:
      r = 8;
      break;

    case 100:
      r = 4;
      break;
      
    case 119:
      r = 2;
      break;
      
    case 115:
      r = 1;
      break;
      
    case 106:
      c = 8;
      break;
      
    case 108:
      c = 4;
      break;
      
    case 105:
      c = 2;
      break;
      
    case 107:
      c = 1; 
      break;
      
    case 113:
      c = 15;   
      break;
      
    default:
      r = 0;
      c = 0;
  }
  
  moveRobot(r);
  moveCamera(c);
}

void moveRobot(int r)
{
  switch (r)
  {
    case 8: //1000 move left
    PORTA = B00001100;
    break;

    case 4: //0100 move right
    PORTA = B00001000;
    break;

    case 2: //0010 move forwards
    PORTA = B00000010;
    break;

    case 1: //0001 move backwards
    PORTA = B00000011;
    break;

    default: //nothing matches
    PORTA = B00000000;  //do nothing
    break;
  }
}

void moveCamera(int c)
{
  switch (c)
    {
      case 15: //1111 center camera
        xPos = xServoCentre;
        yPos = yServoCentre;
        xServo.write(xPos);
        yServo.write(yPos);
        break;
            
      case 8: //1000 look left
        if (xPos > xServoMin)
        {
          xPos = xPos - servoSpd;
          xServo.write(xPos);
          delay(servoDelay); 
        }
        else
        {
          xServo.write(xServoMin);       
          delay(servoDelay);  
        }
        break;
      
      case 4: //0100 look right
        if (xPos < xServoMax)
        {
          xPos = xPos + servoSpd;
          xServo.write(xPos);
          delay(servoDelay); 
        }
        else
        {
          xServo.write(xServoMax);       
          delay(servoDelay);  
        }
        break;
      
      case 2: //0010 look up
        if (yPos < yServoMax)
        {
          yPos = yPos + servoSpd;
          yServo.write(yPos);
          delay(servoDelay); 
        }
        else
        {
          yServo.write(yServoMax);       
          delay(servoDelay);  
        }
        break;
      
      case 1: //0001 look down
        if (yPos > yServoMin)
        {
          yPos = yPos - servoSpd;
          yServo.write(yPos);
          delay(servoDelay); 
        }
        else
        {
          yServo.write(yServoMin);       
          delay(servoDelay);  
        }
        break;
    }  
}
